#ifndef DATA_HOLDER_H_
#define DATA_HOLDER_H_

#define VERSION "v1.0.0"

#include <string.h>
#include <stdio.h>
#include "data_holder_ll.h"

#pragma pack(push)     /* save origin value */
#pragma pack(1)

struct Robot_firmware{
    char version[16]; //固件版本
    char time[16];  //构建时间
};

struct Robot_parameter{
    union{
        char buff[64];
        struct{   
            unsigned short wheel_diameter;   //轮子直径  mm
            unsigned short wheel_track;      //差分：轮距， 三全向轮：直径，四全向：前后轮距+左右轮距 mm
            unsigned short encoder_resolution;  //编码器分辨率
            unsigned char do_pid_interval;         //pid间隔 (ms)
            unsigned short kp;
            unsigned short ki;
            unsigned short kd;
            unsigned short ko;                  //pid参数比例
            unsigned short cmd_last_time;       //命令持久时间ms 超过该时间会自动停止运动
            unsigned short max_v_liner_x;
            unsigned short max_v_liner_y;
            unsigned short max_v_angular_z;
            unsigned char imu_type;
        }buf_struct;
    }body;
};

struct Robot_velocity{
    short v_liner_x; //线速度 前>0 cm/s
    short v_liner_y; //差分轮 为0  cm/s
    short v_angular_z; //角速度 左>0 0.01rad/s  100 means 1 rad/s
};

struct Robot_odom{
    short v_liner_x;      //线速度 前>0 后<0  cm/s
    short v_liner_y;      //差分轮 为0        cm/s
    short v_angular_z;    //角速度 左>0 右<0  0.01rad/s   100 means 1 rad/s
    long x;              //里程计坐标x       cm (这里long为4字节，下同)
    long y;              //里程计坐标y       cm
    short yaw;            //里程计航角       0.1度
};

struct Robot_each_motor{
    union{
        short speed[8];
        struct{
            short vL;   //左电机
            short vR;   //右电机
        }wheels[4];
    }un; 
};

struct Robot_pid_data{
    long input[4];  //各轮子驱动输入值
    long output[4]; //个轮子输出值
};

typedef struct _SetHydrauArm{
    struct{
        uint8_t strech;
        uint8_t drawback;
        uint8_t pump;           //0x00 Off, Other On
    }arms[2];                   //Left or Right
}SetHydrauArm_T;

typedef struct _SetHydrauRaise{
    uint8_t up;
    uint8_t down;
    uint8_t pump;
}SetHydrauRaise_T;

#define TEAM_FRONT 1
#define TEAM_BACK  2

typedef struct {
    short angular_vel;  //角速度，暂时不用
    short wheel_vel;    //电机平均转速
}SetSpin_T;

typedef struct _RobotState{
    
    struct wheel_state{
        uint8_t ready;              //两个驱动是否OK
        double actual_rpm;          //实际转速
        double turns;               //电机圈数
        double odom;                //用电机算得的里程
    };
    
    struct encoder_state{
        uint8_t ready;
        double  turns;                //圈数
        double  odom;                 //用外置编码器算得的里程
    };
    
    struct wheel_state wheels[2];
    struct encoder_state encoder;

}RobotState_T;

/* 设置odom的偏移量，修正odom偏差 */
typedef struct _OdomTf{
    float dx;
    float dy;
    float dth;
}OdomTf_T;

#pragma pack(pop)  /* restore origin value */

class Data_holder{
    public:
        static Data_holder* get(){
            static Data_holder dh;
            return &dh;
        }

        void load_parameter();

        void save_parameter();
    
    private:
        Data_holder(){
            strncpy((char*)&firmware_info.version, VERSION, strlen(VERSION));
            sprintf(firmware_info.time, "%s-m%de%d", "20201122", 15, 19);
            memset(&parameter, 0, sizeof(struct Robot_parameter));
            memset(&velocity, 0, sizeof(struct Robot_velocity));
            memset(&odom, 0, sizeof(struct Robot_odom));
            memset(&pid_data, 0, sizeof(struct Robot_pid_data));
            memset(imu_data, 0, sizeof(imu_data));
            memset(&each_motor, 0, sizeof(Robot_each_motor));
            memset(&hydrau_arms, 0, sizeof(SetHydrauArm_T));
            memset(&robot_spin, 0, sizeof(SetSpin_T));
            memset(&double_car, 0, sizeof(SetDoubleCar_T));
            memset(&odom_tf, 0, sizeof(OdomTf_T));
            }
    public:
        bool power_onoff;
        struct Robot_firmware  firmware_info;
        struct Robot_parameter parameter;
        struct Robot_velocity  velocity;
        struct Robot_odom      odom;
        struct Robot_pid_data  pid_data;
        struct Robot_each_motor each_motor;
        SetHydrauArm_T         hydrau_arms;
        SetHydrauRaise_T       hydrau_raise;
        RobotState_T           robot_state;
        SetSpin_T              robot_spin;
        SetDoubleCar_T         double_car;
        OdomTf_T               odom_tf;

        float imu_data[9];
};
#endif
